import psar as ps
import numpy as np

Fsr = 32.e6
delr = 3.e8 / (2 * Fsr)


r0 = 957190.0
# r0 = 957640.0
r0 = 999798.00309324998
r = r0 + np.linspace(1, 10, 10) * delr

ht = 702682.4128852832
re = 6372006.8787232647
f0, wavl, vel = 178.143433, 0.2360571, 7590.
acc = ps.Ge / ((re + ht)**2)

th = np.arccos(((ht + re)**2 + r * r - re**2) / (2 * r * (re + ht)))

sinsqref = f0 * wavl / (2. * vel * np.sqrt(re / (re + ht)) * np.sin(th))

print(th)
print(sinsqref)


fdr = (2. / wavl) * (acc * np.cos(th) + (vel * sinsqref)**2 - vel**2) / r

veleff = np.sqrt(np.abs(acc * np.cos(th) * r + (vel * sinsqref)**2 - vel**2))

print('fdr: ', fdr)
print('veleff: ', veleff)

i_lrl = -1
r_veln = 7590.0921590946500
r_platvel1, r_platvel2, r_platvel3 = 7590.0820085553305, 3.4106051316484809E-013, 12.413176658646989
r_platacc1, r_platacc2, r_platacc3 = 5.3827226755140956E-003, -0.82296778209009980, -8.1492730997500971

thaz = np.arcsin(((wavl * f0 / (2. * np.sin(th))) + (r_platvel3 / np.tan(th))) /
                 np.sqrt(r_platvel1**2 + r_platvel2**2)) - i_lrl * np.arctan(r_platvel2 / r_platvel1)
r_lookvec1 = np.sin(th) * np.sin(thaz)
r_lookvec2 = np.sin(th) * np.cos(thaz) * i_lrl
r_lookvec3 = -np.cos(th)

r_vdotl = r_lookvec1 * r_platvel1 + r_lookvec2 * r_platvel2 + r_lookvec3 * r_platvel3
r_adotl = r_lookvec1 * r_platacc1 + r_lookvec2 * r_platacc2 + r_lookvec3 * r_platacc3

fdr = 2. * (r_adotl + (r_vdotl**2 - r_veln**2) / r) / (wavl)
veleff = np.sqrt(-(r_adotl * r + r_vdotl**2 - r_veln**2))

print('fdr: ', fdr)
print('veleff: ', veleff)

